/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.engcomp.mpc.domain;

import br.ufrn.engcomp.mpc.Constants;
import br.ufrn.engcomp.mpc.util.MathCommonsUtils;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.RealMatrix;

/**
 *
 * @author Victor
 */
public class DisturbObserver {

    private Plant plant;
    private RealMatrix lMatrix;
    private RealMatrix fMatrix;

    public DisturbObserver(Plant plant) {
        this.plant = plant;
        
        fMatrix = new Array2DRowRealMatrix(new double[] { 0, 1 });
        lMatrix = new Array2DRowRealMatrix(new double[] { 0.0, 0.7 }).transpose();
    }

    
    public double computeDisturb(double[] lastX, double[] nextX, double[] lastU, double lastDisturb) {
        RealMatrix I = MathCommonsUtils.eye(Constants.M);
        RealMatrix lastXAsMatrix = new Array2DRowRealMatrix(lastX);
        RealMatrix nextXAsMatrix = new Array2DRowRealMatrix(nextX);
        RealMatrix lastUAsMatrix = new Array2DRowRealMatrix(lastU);
        // I - LF
        RealMatrix firstTerm = I.subtract(lMatrix.multiply(fMatrix));
        // Ax(k-1)
        RealMatrix secondTerm = plant.getAMatrix().multiply(lastXAsMatrix);
        // Bu(k-1)
        RealMatrix thirdTerm = plant.getBMatrix().multiply(lastUAsMatrix);

        // (I-LF)*d_ant + L*(x(k) - Ax(k-1) - Bu(k-1))
        double disturb = firstTerm.scalarMultiply(lastDisturb).add(lMatrix.multiply(nextXAsMatrix.subtract(secondTerm).subtract(thirdTerm))).getEntry(0, 0);

        // d(k)
        return disturb;
    }
    
    public void turnOff(){
        System.out.println("Estabilizou");
    }

    // Sistema linear a ser resolvido para determinar o deltaU
    // B * uEst + (1 - A) * xEst = -F * dEst
    // C * xEst = SetPoint
    public double[] solveAXb(double disturb, double setPoint) {
        RealMatrix sysA = new Array2DRowRealMatrix(3, 3);
        RealMatrix sysB = new Array2DRowRealMatrix(3, 1);
        RealMatrix I = MathCommonsUtils.eye(2);

        // sysA = [I - A, B; C, 0]
        sysA.setSubMatrix(I.subtract(plant.getAMatrix()).getData(), 0, 0); // I - A
        sysA.setSubMatrix(plant.getBMatrix().getData(), 0, 2); // B
        sysA.setSubMatrix(plant.getCMatrix().getData(), 2, 0); // C
        //sysA.setEntry(2, 2, 0);
        
        // sysB = [-Fd setPoint]'
        sysB.setSubMatrix(fMatrix.scalarMultiply(-disturb).getData(), 0, 0); // -F * dEst
        sysB.setEntry(2, 0, setPoint);

//        return MathCommonsUtils.solveAXb(sysA, sysB).getEntry(2);
        return MathCommonsUtils.solveAXb(sysA, sysB).getData();
    }
}